/**
 * @file HybridAStarPlanner.h
 * @brief 混合A*算法路径规划器
 * @details
 * Hybrid A*算法原理：
 * 1. 在连续状态空间(x,y,θ)中搜索，而不是离散网格
 * 2. 使用车辆运动模型生成可行路径
 * 3. 结合图搜索和运动规划
 * 4. 使用Reeds-Shepp曲线作为启发式函数
 * 5. 考虑车辆运动学约束
 *
 * 适用场景：车辆路径规划，考虑转向半径约束
 */

#ifndef HYBRID_ASTAR_PLANNER_H
#define HYBRID_ASTAR_PLANNER_H

#include "core/PlannerBase.h"
#include <queue>
#include <unordered_map>
#include <cmath>

namespace PathPlanning
{
/**
 * @brief Hybrid A*算法配置参数
 */
struct HybridAStarConfig : public PlannerConfig
{
    double stepSize         = 1.0; // 运动步长
    double maxSteeringAngle = 0.6; // 最大转向角 (弧度)
    double wheelbase        = 2.5; // 车辆轴距
    int    numThetaBins     = 72;  // 朝向离散化数量

    // 代价参数
    double turningPenalty   = 0.5;
    double reversingPenalty = 1.0;
    double obstaclePenalty  = 10.0;

    // 目标容忍度
    double goalPositionTolerance = 0.5;
    double goalAngleTolerance    = 0.2;

    HybridAStarConfig()
    {
        plannerName = "HybridAStar";
    }
};

/**
 * @brief 控制指令
 */
struct Control
{
    double steeringAngle; // 转向角
    double stepLength;    // 步长
    bool   forward;       // 前进/后退

    Control(double angle = 0, double length = 0, bool fwd = true)
        : steeringAngle(angle), stepLength(length), forward(fwd)
    {
    }
};

/**
 * @brief Hybrid A*搜索节点
 */
struct HybridAStarNode : public Node
{
    Pose    pose;      // 位姿 (x, y, theta)
    Control motion;    // 到达此节点的运动指令
    bool    direction; // 行驶方向

    HybridAStarNode(const Pose& pose) : pose(pose)
    {
        position = Point(pose.x, pose.y);
    }
};

/**
 * @brief 离散化状态 (用于查重)
 */
struct HybridState
{
    int x, y, thetaBin;

    HybridState(int x = 0, int y = 0, int theta = 0) : x(x), y(y), thetaBin(theta) {}

    bool operator==(const HybridState& other) const
    {
        return x == other.x && y == other.y && thetaBin == other.thetaBin;
    }
};

/**
 * @brief 状态哈希函数
 */
struct HybridStateHash
{
    std::size_t operator()(const HybridState& state) const
    {
        return std::hash<int>()(state.x) ^ (std::hash<int>()(state.y) << 1) ^ (std::hash<int>()(state.thetaBin) << 2);
    }
};

/**
 * @brief Hybrid A*节点比较器
 */
struct HybridAStarNodeCompare
{
    bool operator()(const std::shared_ptr<HybridAStarNode>& a, const std::shared_ptr<HybridAStarNode>& b) const
    {
        return a->fCost > b->fCost;
    }
};

class HybridAStarPlanner : public PlannerBase
{
public:
    HybridAStarPlanner(std::shared_ptr<Map> map);

    PlanningResult plan(const Point& start, const Point& goal) override;

private:
    /**
     * @brief 生成运动基元
     */
    std::vector<Control> generateMotionPrimitives();

    /**
     * @brief 应用运动模型
     */
    std::shared_ptr<HybridAStarNode> applyMotionModel(const HybridAStarNode* current, const Control& control);

    /**
     * @brief 碰撞检测
     */
    bool isCollisionFree(const HybridAStarNode* from, const HybridAStarNode* to);

    /**
     * @brief 计算启发式函数值
     */
    double calculateHeuristic(const HybridAStarNode* node, const Point& goal);

    /**
     * @brief 离散化状态
     */
    HybridState discretizeState(const Pose& pose);

    /**
     * @brief 检查是否到达目标
     */
    bool isGoalReached(const HybridAStarNode* node, const Point& goal);

    /**
     * @brief 重建路径
     */
    std::vector<Pose> reconstructPath(const std::shared_ptr<HybridAStarNode>& endNode);
};

} // namespace PathPlanning

#endif